/*
 * mpu6050.c
 *
 *  Created on: Jul 27, 2015
 *      Author: kench
 */


#include "MPU6050.h"

uint8_t mpu6050_buffer[14];
//extern struct _sensor sensor;




uint8_t Init_MPU6050(){
	uint8_t ack;
	//delay
	//ack = Single_Read(MPU6050_ADDRESS, WHO_AM_I);
	//if (!ack)
	//	return ERROR;
	I2cWrite(W_MPU6050_ADDRESS,PWR_MGMT_1,  0x00);	
	I2cWrite(W_MPU6050_ADDRESS,SMPLRT_DIV, 	0x07);
	I2cWrite(W_MPU6050_ADDRESS,GYRO_CONFIG, 0x10);
	I2cWrite(W_MPU6050_ADDRESS,ACCEL_CONFIG,0x09);
	I2cWrite(W_MPU6050_ADDRESS,CONFIG,	0x03);
	
	return OK;
}

void MPU6050_Read(){
	I2cRead(W_MPU6050_ADDRESS,ACCEL_XOUT_H,14,mpu6050_buffer);	
}

void MPU6050_Dataanl(void){
	MPU6050_Read();
	
	sensor.acc.origin.x = ((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - sensor.acc.quiet.x;
	sensor.acc.origin.y = ((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - sensor.acc.quiet.y;
	sensor.acc.origin.z = ((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]);

	sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
	sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
	sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
  
	sensor.gyro.radian.x = sensor.gyro.origin.x - sensor.gyro.quiet.x;
	sensor.gyro.radian.y = sensor.gyro.origin.y - sensor.gyro.quiet.y;
	sensor.gyro.radian.z = sensor.gyro.origin.z - sensor.gyro.quiet.z;
	
}



//const uint8_t MPU6050InitDat[][2]={
//{MPU6050_REG_PM1   ,0b00000001},
//				 /*   |||||***------CLKSEL = 001:PLL with X axis gyroscope reference
//				      ||||*---------TEMP_DIS 0  :enable the temperature sensor
//				      |||*----------always 0
//				      ||*-----------CYCLE =  0  :disable cycle
//				      |*------------SLEEP =  0  :out sleep mode
//				      *-------------DEVICE_RESET = 0: 
//				*/
//{MPU6050_REG_USERC ,0b00000000},
//				/*    |||||||*------SIG_COND_RESET = 0:don't reset the sensor
//				      ||||||*-------I2c_MST_RESET  = 0:don't reset the iic master
//				      |||||*--------FIFO_RESET     = 0;don't reset the FIFO
//				      |||**---------always 0
//				      ||*-----------I2c_MST_EN =  0  :disable IIC master
//				      |*------------FIFO_EN =  0  :disable the FIFO
//				      *-------------always 0 
//				*/
//{MPU6050_REG_CONFIG,0b00000010},
//				/*    |||||***------DLPF_CFG = 0b010 :Accelerometer Bandwidth= 94HZ Gyroscope = 98HZ sample=1Khz
//				      ||***---------EXT_SYNC_SET  = 0b00:Input disabled
//				      **------------always 0
//				*/
//{MPU6050_REG_SMPRT ,0b00011001/*SAMPLE_RATE*/},//δУ׼
//				/*    ********------SMPLRT_DIV = 2:Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)=1K/4=250HZ
//				*/
//{MPU6050_REG_GYROC ,0b00011000},
//				/*    |||||***------always 0
//				      |||**---------FS_SEL  = 11:Full Scale Range = �ҩ� 2000 �ҧ�/s
//				      ||*-----------ZG_ST   = 0 :Z axis gyroscope don't self test
//				      |*------------YG_ST   = 0 :Y axis gyroscope don't self test
//				      *-------------XG_ST   = 0 :X axis gyroscope don't self test
//				*/
//{MPU6050_REG_ACCELC,0b00011000},
//				/*    |||||***------always 0
//				      |||**---------FS_SEL  = 11:Full Scale Range = 8g
//				      ||*-----------ZA_ST   = 0 :Z axis accelerometer don't self test
//				      |*------------YA_ST   = 0 :Y axis accelerometer don't self test
//				      *-------------XA_ST   = 0 :X axis accelerometer don't self test
//				*/
//{MPU6050_REG_INTP_CFG,0b10010000},/*set as pulse and read clear mode*/
//				/*      |||||||*------ 0 
//				        ||||||*-------I2c_BYPASS_EN  =0   0 disable 1 enable
//				        |||||*--------FSYNC_INT_EN   =0   0 disable 1 enable
//				        ||||*---------FSYNC_INT_LEVEL=0   0 disable 1 enable
//				        |||*----------INT_RD_CLEAR   =1   0 disable  1 enable
//				        ||*-----------LATCH_INT_EN   =0   0 pulse interrupt 1 level
//				        |*------------INT_OPEN       =0   0 push pull 1 open drain
//				        *-------------INT_LEVEL      =1   0 active h 1 active l
//				*/
//
//{MPU6050_REG_INTP_EN, 0b00000001},/*enable data ready interrput*/
//		/*      |||||||*------DATA_RDY_EN  =1    0 disable 1 enable
//		        ||||||*-------0
//		        |||||*--------0
//		        ||||*---------I2c_MST_INT_EN=0   0 disable 1 enable
//		        |||*----------FIFO_OFLOW_EN =0   0 disable  1 enable
//		        ||*-----------0
//		        |*------------MOT_EN        =0   0 disable  1 enable
//		        *-------------0
//		*/
//
//
//{0xff,0xff}
//};
//uint8_t Init_MPU6050(void){
//	uint8_t i;
//	const uint8_t *p;
//	R_IICA0_Create();
//	if(OK!=Test_MPU6050())	return ERROR;
//	p=&(MPU6050InitDat[0][0]);
//	while(*p!=0xff)
//	{
//		if(OK!=Write_MPU6050(*p,(p+1),1))return ERROR;
//		p+=2;
//	}
//	return OK;
//}
//
//uint8_t Get_MPU6050_Dat_H(struct mpu6050st *datbuf)
//{
//	uint8_t i,*p,j;
//	//if(OK != Read_MPU6050(MPU6050_REG_SENSOR_DAT,(uint8_t*)(datbuf),sizeof(struct mpu6050st)))
//	{
//		//R_IICA0_Create();
//		return ERROR;
//	}
//	p = (uint8_t*)(datbuf);
//	
//	/*for(i=0;i<sizeof(struct mpu6050st);i+=2)
//	{
//		j=*p;
//		*p=*(p+1);
//		*(p+1)=j;
//		p+=2;
//	}*/
//	return OK;
//}
//
//
//static uint8_t Sel_MPU6050_Reg(uint8_t reg){
//	if(OK != I2c_start())	return ERROR;
//	if(OK!= I2c_senddat(W_MPU6050_ADDRESS))	{
//		I2c_stop();
//		return ERROR;
//	};
//	if(OK!=I2c_senddat(reg)){
//		I2c_stop();
//		return ERROR;
//	};
//	return OK;
//}
//static uint8_t Write_MPU6050(uint8_t reg,uint8_t *datbuf,uint16_t datl){
//	if(OK!=Sel_MPU6050_Reg(reg))return ERROR;
//	for(;datl!=0;datl--)
//	{
//		if(OK!=I2c_senddat(*datbuf))	{
//			I2c_stop();
//			return ERROR;
//		};
//		datbuf++;
//	}
//	return I2c_stop();
//}
//static uint8_t Read_MPU6050(uint8_t reg, uint8_t *datbuf, uint16_t datl){
//	if(OK!=Sel_MPU6050_Reg(reg))return ERROR;
//	if(OK!=I2c_rstart())return ERROR;
//	if(OK!=I2c_senddat(R_MPU6050_ADDRESS))
//	{
//		I2c_stop();
//		return ERROR;
//	};
//
//	for(;datl!=0;datl--){
//		*datbuf=I2c_recedat(datl);
//		datbuf++;
//	}
//	return I2c_stop();
//}
//
//
//static uint8_t Test_MPU6050(void){
//	uint8_t buff[2]={0,0};
//	Read_MPU6050(MPU6050_REG_WHOAMI,buff,1);
//	buff[0]&=0x7e;
//	if(buff[0]==0x68)return OK;
//	return ERROR;
//}
//
